"""
--------------------------------------------------------------------------------
功能:通过socket获取手持陀螺仪模块数据,并控制云台运动
日期:2025-06-03
作者:Unirobot
QQ交流群:811348489
寄语：代码中能遇见你，真好，有你，真好.
署名：有你同创智能机器人科技(北京)有限公司
--------------------------------------------------------------------------------
"""

import socket
import json
import threading
import queue
import math
import time
import numpy as np
from src.arm_control import custom_font
from src.arm_control import manager
from src.arm_control import process_gyro_data

process_gyro = process_gyro_data.ProcessGyro()
# 创建线程间通信的队列
arm_command_queue = queue.Queue(maxsize=1)  # 限制队列大小防止积压

def arm_worker():
    global roll_windows, pitch_windows, last_yaw_data, loop_left, loop_right, loop_shows
    global left_numbers,gyro_turn_left_flag,gyro_turn_right_flag, right_numbers, loop_numbers
    """机械臂控制线程"""
    node = manager.RobotManager()
    input_point = 1
    while True:
        command = arm_command_queue.get()  # 获取包含两个参数的元组
        try:
            pitch, roll = command
            
            # 原有控制逻辑
            if roll >= 35:
                node.gimbal_ang[0] -= 4
                input_point = 1
            elif roll <= -35:
                node.gimbal_ang[0] += 4
                input_point = 1
            elif pitch >= 30:
                node.gimbal_ang[1] += 4
                input_point = 1
            elif pitch <= -30:
                node.gimbal_ang[1] -= 4
                input_point = 1
            node.gimbal_move(node.gimbal_ang[0], node.gimbal_ang[1], input_point)

        except Exception as e:
            print(f"[ARM ERROR] 机械臂控制错误: {e}")

# 启动机械臂控制线程
arm_thread = threading.Thread(target=arm_worker, daemon=True)
arm_thread.start()

# 主线程处理陀螺仪数据
node = manager.RobotManager()
font = custom_font.CustomFont()
print(font.color_text(font.compony_msg, color="cyan", bold=True))

HOST = '0.0.0.0'
PORT = 9000
print(f"[INFO] 启动Socket服务端, 监听端口 {PORT} ...")

with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
    s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    s.bind((HOST, PORT))
    s.listen(1)
    print("[INFO] 等待Arduino客户端连接...")
    conn, addr = s.accept()
    with conn:
        print(f"[INFO] 已连接: {addr}")
        buffer = b''
        while True:
            data = conn.recv(1024)
            if not data:
                print("[WARN] 客户端断开连接")
                break
            buffer += data
            while b'\n' in buffer:
                line, buffer = buffer.split(b'\n', 1)
                try:
                    sensor_data = json.loads(line.decode('utf-8'))
                    
                    # 原有角度计算
                    if not process_gyro.calibration_complete:
                        process_gyro.calibration_gyro(sensor_data['accel'])
                    else:
                        data = time.time()
                        roll, pitch, yaw, time_delay= process_gyro.calculate_angles_back(sensor_data['accel'], sensor_data['gyro'], data)
                        # 非阻塞方式发送命令到机械臂线程
                        if arm_command_queue.empty():
                            # 添加位移数据到队列
                            arm_command_queue.put_nowait((
                                pitch, roll
                            ))
                except Exception as e:
                    print(f"[ERROR] 解码失败: {e}")